//
// Created by fazhehy on 2024/8/14.
//

#include "task_outdoor_ensure.h"

servo_param_t outdoor_ensure_gnss, outdoor_ensure_sound;
static float distance_range = 6;
uint8_t outdoor_ensure_config(uint8_t keyNum)
{
    static uint8_t times = 0, pos = 0, isSelected = 0;
    static uint16_t x = 0, y = 0, len = 0;

    if (times == 0){
        pos = 0;
        isSelected = 0;
        x = DISPLAY_OFFSET_X;
        y = DISPLAY_OFFSET_Y+pos*20;
        len = 10*FONT_WIDTH+ROUND_RECT_R;
        snprintf(str[0], 64, "ensure run");
        snprintf(str[1], 64, "gnss.speed   ----->%5d",   outdoor_ensure_gnss.speed);
        snprintf(str[2], 64, "gnss.servo.p ----->%6.3f", outdoor_ensure_gnss.p);
        snprintf(str[3], 64, "gnss.servo.i ----->%6.3f", outdoor_ensure_gnss.i);
        snprintf(str[4], 64, "gnss.servo.d ----->%6.3f", outdoor_ensure_gnss.d);
        snprintf(str[5], 64, "sound.speed  ----->%5d",   outdoor_ensure_sound.speed);
        snprintf(str[6], 64, "sound.servo.p----->%6.3f", outdoor_ensure_sound.p);
        snprintf(str[7], 64, "sound.servo.i----->%6.3f", outdoor_ensure_sound.i);
        snprintf(str[8], 64, "sound.servo.d----->%6.3f", outdoor_ensure_sound.d);
        snprintf(str[9], 64, "distance range----->%.3f", distance_range);
        for (int i = 0; i < 10; ++i) {
            st7789_show_string(2, i * 20 + DISPLAY_OFFSET_Y + 2, str[i]);
        }
        times = 1;
        *p_indoor_flag = false;
        SCB_CleanInvalidateDCache();
    }

    if (keyNum != NOP  && isSelected == 0)
        st7789_draw_round_rectangle(x, y, len, FONT_HEIGHT+4, ROUND_RECT_R, ST7789_BG_COLOR);

    if (keyNum == UP || keyNum & ENCODER_POS_PREFIX){
        if (isSelected == 0){
            if (pos != 0)
                pos --;
        }
        else{
            switch (pos) {
                case 1: outdoor_ensure_gnss.speed += 50;break;
                case 2: outdoor_ensure_gnss.p += 1.0f;break;
                case 3: outdoor_ensure_gnss.i += 0;break;
                case 4: outdoor_ensure_gnss.d += 1.0f;break;
                case 5: outdoor_ensure_sound.speed += 10;break;
                case 6: outdoor_ensure_sound.p += 1.0f;break;
                case 7: outdoor_ensure_sound.i += 0;break;
                case 8: outdoor_ensure_sound.d += 1.0f;break;
                case 9: distance_range += 0.5f;break;
                default:break;
            }
        }
    }
    else if (keyNum == DOWN || keyNum & ENCODER_NEG_PREFIX){
        if (isSelected == 0){
            if (pos != 9)
                pos ++;
        }
        else{
            switch (pos) {
                case 1: outdoor_ensure_gnss.speed -= 50;break;
                case 2: outdoor_ensure_gnss.p -= 1.0f;break;
                case 3: outdoor_ensure_gnss.i -= 0;break;
                case 4: outdoor_ensure_gnss.d -= 1.0f;break;
                case 5: outdoor_ensure_sound.speed -= 10;break;
                case 6: outdoor_ensure_sound.p -= 1.0f;break;
                case 7: outdoor_ensure_sound.i -= 0;break;
                case 8: outdoor_ensure_sound.d -= 1.0f;break;
                case 9: distance_range -= 0.5f;break;
                default:break;
            }
        }
    }
    else if (keyNum == ENTER){
        if (pos != 0){
            if (isSelected == 1)
                isSelected = 0;
            else{
                isSelected = 1;
                x = DISPLAY_OFFSET_X;
                y = DISPLAY_OFFSET_Y+pos*20;
                len = strlen(str[pos])*FONT_WIDTH+ROUND_RECT_R;

                st7789_draw_round_rectangle(x, y, len, FONT_HEIGHT+4, ROUND_RECT_R, ST7789_PEN_COLOR);
            }
        }
        else{
            times = 0;
            return ENTER;
        }
    }
    else if (keyNum == BACK){
        times = 0;
        return BACK;
    }

    st7789_printf(40, 20, "isSelected:%d", isSelected);
    if (isSelected == 1){
        switch (pos) {
            case 1:snprintf(str[1], 64, "gnss.speed   ----->%5d",   outdoor_ensure_gnss.speed);break;
            case 2:snprintf(str[2], 64, "gnss.servo.p ----->%6.3f", outdoor_ensure_gnss.p);break;
            case 3:snprintf(str[3], 64, "gnss.servo.i ----->%6.3f", outdoor_ensure_gnss.i);break;
            case 4:snprintf(str[4], 64, "gnss.servo.d ----->%6.3f", outdoor_ensure_gnss.d);break;
            case 5:snprintf(str[5], 64, "sound.speed  ----->%5d",   outdoor_ensure_sound.speed);break;
            case 6:snprintf(str[6], 64, "sound.servo.p----->%6.3f", outdoor_ensure_sound.p);break;
            case 7:snprintf(str[7], 64, "sound.servo.i----->%6.3f", outdoor_ensure_sound.i);break;
            case 8:snprintf(str[8], 64, "sound.servo.d----->%6.3f", outdoor_ensure_sound.d);break;
            case 9:snprintf(str[9], 64, "distance range----->%.3f", distance_range);break;
            default:
                break;
        }
        st7789_show_string(2, pos * 20 + DISPLAY_OFFSET_Y + 2, str[pos]);
    }

    if (keyNum != NOP && isSelected == 0){
        x = DISPLAY_OFFSET_X;
        y = DISPLAY_OFFSET_Y+pos*20;
        len = strlen(str[pos])*FONT_WIDTH+ROUND_RECT_R;

        st7789_draw_round_rectangle(x, y, len, FONT_HEIGHT+4, ROUND_RECT_R, ST7789_PEN_COLOR);
    }

    return NO_SHOW;
}

uint8_t outdoor_ensure_run(uint8_t keyNum)
{
    float distance = 10, angle = 0;

    float yaw = 0;
    float target_angle = 0;

    bool sound_track_flag = false;
    bool update_flag = false;
    bool brake_flag = false;
    bool first_flag = true;

    int16_t duty = 0;

    uint8_t now_point = 0;
    uint8_t last_num = 0;


    servo_init();
    position_pid_init(&servo_pid, outdoor_ensure_gnss.p, outdoor_ensure_gnss.i, outdoor_ensure_gnss.d, SERVO_LIMIT, 300);
    servo_set_duty(SERVO_MEDIAN);

    while (lora_receive_num == 0){
        st7789_printf(40, 0, "Ready!!!!");
    }
    st7789_printf(40, 0, "Start!!!!");

    now_point = lora_receive_num;
    last_num = now_point;

    *p_target_bldc_duty = outdoor_ensure_gnss.speed;
    SCB_CleanInvalidateDCache();

    while (1){
        if (!sound_track_flag){

            now_point = lora_receive_num;
            if (now_point == 0){
                break;
            }

            if (brake_flag){
                SCB_CleanInvalidateDCache();
                if (*p_bldc_speed < 30000){
                    system_delay_ms(300);
                    position_pid_init(&servo_pid, -outdoor_precise_sound.p, -outdoor_precise_sound.i, -outdoor_precise_sound.d, SERVO_LIMIT, 300);
                    *p_target_bldc_duty = outdoor_precise_sound.speed;
                    SCB_CleanInvalidateDCache();
                    sound_track_flag = true;
                    update_flag = false;
                    continue;
                }
            }

            if (gnss_flag == 1){
                gnss_data_parse();
                distance = (float )get_two_points_distance(gnss.latitude, gnss.longitude,gnss_target_point[now_point].latitude, gnss_target_point[now_point].longitude);
                angle = (float )get_two_points_azimuth(gnss.latitude, gnss.longitude,gnss_target_point[now_point].latitude, gnss_target_point[now_point].longitude);
                gnss_flag = 0;
                update_flag = true;
            }

            yaw = convert_angle(eulerAngle.yaw);
            target_angle = (float )servo_input_angle(angle,yaw);
            if (!brake_flag){
                duty = (int16_t)position_pid(&servo_pid, target_angle, 0);
                servo_set_duty((int16_t)(duty+SERVO_MEDIAN));
            }
            else{
                servo_set_duty(SERVO_MEDIAN);
            }

            if (update_flag){
                if (distance <= distance_range){
                    if (first_flag)
                        *p_target_bldc_duty = outdoor_precise_sound.speed;
                    else{
                        if (!brake_flag){
                            servo_set_duty(SERVO_MEDIAN);
                            *p_target_bldc_duty = 0;
                            SCB_CleanInvalidateDCache();
                        }
                        else{
                            *p_target_bldc_duty = outdoor_precise_sound.speed;
                            SCB_CleanInvalidateDCache();
                        }
                    }
                    brake_flag = true;
                }
                else{
                    if (fabsf(target_angle) > 70){
                        *p_target_bldc_duty = (int16_t )(800 - 800*0.5f*fabsf(target_angle)/180);
                    }
                    else{
                        *p_target_bldc_duty = (int16_t )((float )outdoor_ensure_gnss.speed - (float )outdoor_ensure_gnss.speed*0.5f*fabsf(target_angle)/180);
                    }
                    SCB_CleanInvalidateDCache();
                }
                update_flag = false;
            }
        }
        else{
            now_point = lora_receive_num;
            if (now_point == 0){
                break;
            }

            if (now_point != last_num){

                sound_track_flag = false;
                brake_flag = false;
                first_flag = false;

                position_pid_init(&servo_pid, outdoor_precise_gnss.p, outdoor_precise_gnss.i, outdoor_precise_gnss.d, SERVO_LIMIT, 300);

                if (gnss_flag == 1){
                    gnss_data_parse();
                    gnss_target_point[last_num].longitude = gnss.longitude;
                    gnss_target_point[last_num].latitude = gnss.latitude;
                    gnss_flag = 0;
                }
                *p_target_bldc_duty = outdoor_precise_gnss.speed;
                SCB_CleanInvalidateDCache();
                last_num = now_point;
            }
            else{

                SCB_CleanInvalidateDCache();
                target_angle = (float )m7_1_sound_position[1];

                if (fabsf(target_angle) < 90){
                    if (*p_target_bldc_duty != outdoor_precise_sound.speed){
                        *p_target_bldc_duty = outdoor_precise_sound.speed;
                        SCB_CleanInvalidateDCache();
                    }
                }
                else if (fabsf(target_angle) >= 90){
                    if (*p_target_bldc_duty != -outdoor_precise_sound.speed){
                        *p_target_bldc_duty = -outdoor_precise_sound.speed;
                        SCB_CleanInvalidateDCache();
                    }
                }

                if (target_angle > 90) {
                    duty = (int16_t) position_pid(&servo_pid, target_angle, 180);
                    servo_set_duty((int16_t)(duty+SERVO_MEDIAN));
                }
                else if (target_angle < -90) {
                    duty = (int16_t) position_pid(&servo_pid, target_angle, -180);
                    servo_set_duty((int16_t)(duty+SERVO_MEDIAN));
                }
                else {
                    duty = (int16_t) position_pid(&servo_pid, target_angle, 0);
                    servo_set_duty((int16_t)(duty+SERVO_MEDIAN));
                }

            }
        }

        st7789_printf(0, 40, "position:%4d\nduty:%d\n", m7_1_sound_position[1], duty);
        st7789_printf(0, 80, "ang:%f\ndis:%f\nyaw:%.3f\n", angle, distance, yaw);
    }

    servo_set_duty(SERVO_MEDIAN);
    *p_target_bldc_duty = 0;
    SCB_CleanInvalidateDCache();
    motor_stop();
    beep_off();
    return BACK;
}
